﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Diagnostics;

namespace VRP_VRTR_by_Will_and_Ying
{
    class VRTR
    {
        public static double record;
        public static double deviation;

        // Overall bestSolution among three best solutions generated from three different initial solutions
        public static CVRPSolution bestSolution;
        // currentBestSolution generated from the current initial solution
        public static CVRPSolution currentBestSolution;

        public static bool recordUpdate;
        public static bool moveMade;

        // A dictionary to store the neighbors of each customer node
        public static CVRPData vrpDataTest;
        public static Dictionary<Node, Dictionary<Node,int>> neighborList;
        public static List<Node> nodeList;
        public static Link[,] linkMatrix;
        public static double capacity;

        public static CVRPSolution Solve(CVRPData vrpData)
        {
            vrpDataTest = vrpData;
            // Initialization
            int I = 30;
            int K = 5;
            int P = 1;
            double alpha = 1.0;
            int fixedNumOfNeighbors = 40;
            capacity = vrpData.Capacity; 

            List<double> lambdaList = new List<double>() { 0.6, 0.8, 1.0, 1.2, 1.4, 1.6 };
            bestSolution = null;

            double timepassed = 0;
            while (timepassed <= 5)
            {
                // Time limitation
                Stopwatch timer = new Stopwatch();            // get running time
                timer.Start();                                // start to calculate time    

                foreach (double lambda in lambdaList)
                {
                    // Starting solution
                    vrpData.CreateNodeList();
                    vrpData.CreateLinkMatrix();
                    nodeList = vrpData.NodeList; //example1.vrp 0-->21, 0 is depot
                    linkMatrix = vrpData.LinkMatrix;

                    ///////////////////////////////////////////////////////////////////
                    // Initialize the neighbor list for each node i
                    neighborList = new Dictionary<Node, Dictionary<Node, int>>();
                    for (int i = 1; i < nodeList.Count; i++)
                    {
                        List<Link> ListofNeighborLink = new List<Link>();
                        for (int j = 1; j < nodeList.Count; j++)
                        {
                            if (j != i)
                            {
                                ListofNeighborLink.Add(new Link(nodeList[i], nodeList[j]));
                            }
                        }


                        // Sort the neighbor link list of node i in ascending order
                        var sortedLengthList = from element in ListofNeighborLink
                                               orderby element.Length ascending
                                               select element;

                        int count = 0;
                        Dictionary<Node, int> neighbors = new Dictionary<Node, int>();
                        foreach (Link Lij in sortedLengthList)
                        {
                            neighbors.Add(Lij.Head, 0);
                            count++;
                            if (count >= alpha * fixedNumOfNeighbors)
                            {
                                break;
                            }
                        }
                        // Store neighbor list for node i 
                        neighborList.Add(nodeList[i], neighbors);
                    }

                    //////////////////////////////////////////////////////////////////


                    CVRPSolution currentSolution = SavingsMethod.Solve(vrpData, lambda);
                    record = currentSolution.TotalCost;
                    deviation = 0.01 * record;
                    // Initialize the currentBestSolution for the current lambda
                    currentBestSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentSolution);

                    // Set iteration parameter
                    int p = 0;
                    int k = 0;

                    while (p < P)
                    {
                        // Improve the current solution, diversification phase
                        for (int i = 0; i < I; i++)
                        {
                            moveMade = false;

                            // Apply the following move mechanism, true means record-to-record move
                            OnePointMove(currentSolution, true, true, false);
                            
                            TwoOptMove(currentSolution, true, true, true);
                            //TwoPointMove(currentSolution, true, true, false);
                            //TwoPointMove(currentSolution, true, true, true);

                            // If no feasible record-to-record travel move is made, break the loop
                            if (moveMade == false)
                            {
                                break;
                            }

                            // bestCurrentSolution is updated during the move, now update the record
                            UpdateRecord();

                        }

                        // Apply the following move mechanism, false means only downhill moves are allowed
                        // Intensification phase
                        moveMade = true;
                        while (moveMade == true)
                        {
                            moveMade = false;
                            OnePointMove(currentSolution, false, true, true);
                            
                            TwoOptMove(currentSolution, false, true, false);
                            TwoOptMove(currentSolution, false, true, true);
                            //TwoPointMove(currentSolution, false, true, false);
                        }

                        // Update currentBestSolution
                        UpdateCurrentBestSolution(currentSolution);

                        // Update the record again
                        UpdateRecord();

                        // If record is updated during the neighbor search, set k = 0 so that at least K more iterations could be performed.
                        if (recordUpdate == true)
                        {
                            k = 0;
                        }

                        k++;

                        if (k == K)
                        {
                       
                            UpdateCurrentBestSolution(currentSolution);
                            p++;
                        }
                    }

                    if (bestSolution == null)
                    {
                        bestSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentBestSolution);
                    }
                    else
                    {
                        if (currentBestSolution.TotalCost < bestSolution.TotalCost)
                        {
                            bestSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentBestSolution);
                        }
                    }
                }
                timer.Stop();                                                                       // stop to calculate run time
                double totalTime = (timer.ElapsedMilliseconds) / 60000.0;                          // get the elapsed time as a TimeSpan value
                timepassed = timepassed + totalTime;
            }
        
            return bestSolution;
        }

        public static void UpdateRecord()
        {
            // First set recordUpdate = false
            recordUpdate = false;

            // Update the record and deviation if possible
            if (currentBestSolution.TotalCost < record)
            {
                record = currentBestSolution.TotalCost;
                deviation = 0.01 * record;
                recordUpdate = true;
            }
        }

        public static void UpdateCurrentBestSolution(CVRPSolution currentSolution)
        {
            if (currentSolution.TotalCost < currentBestSolution.TotalCost)
            {
                currentBestSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentSolution);
            }
        }

        // calculate the cost of removing one node from the route
        public static double OnePointRemoveCost(int indexOfRemoveNode, Path route)
        {
            double removeCost = 0;
            int i = indexOfRemoveNode;
            removeCost = linkMatrix[route.AllNodes[i - 1].Id, route.AllNodes[i + 1].Id].Length
                - linkMatrix[route.AllNodes[i - 1].Id, route.AllNodes[i].Id].Length
                - linkMatrix[route.AllNodes[i].Id, route.AllNodes[i + 1].Id].Length;
            return removeCost;
        }

        // calculate the cost of inserting insertNode between the edge (tail, head)
        public static double OnePointInsertCost(Node insertNode, Node tail, Node head)
        {
            double insertCost = 0;
            insertCost = linkMatrix[tail.Id, insertNode.Id].Length + linkMatrix[insertNode.Id, head.Id].Length
                - linkMatrix[tail.Id, head.Id].Length;
            return insertCost;
        }

        public static void MakeOnePointIntraRouteMove(Node insertNode, Node tail, Node head, Path route, CVRPSolution solution)
        {
            double originalRouteLength = route.Length;

            route.AllNodes.Remove(insertNode);
            // why getting index of the tail node instead of the head node, because head node might be depot,
            // in this case, the head is actually the last node of the route, remember that the first node of the route
            // is also depot, so the returned index will be 0, it causes an error! 
            int index = route.AllNodes.IndexOf(tail);
            route.AllNodes.Insert(index + 1, insertNode);
            route.CalculateLength();
            solution.TotalCost = solution.TotalCost + route.Length - originalRouteLength;
        }

        public static void MakeOnePointInterRouteMove(Node insertNode, Node tail, Node head, Path routeOfInsertNode, Path targetRoute, CVRPSolution solution)
        {
            double originalRouteOfInsertNodeLength = routeOfInsertNode.Length;
            double originalTargetRouteLength = targetRoute.Length;

            if (routeOfInsertNode.AllNodes.Count == 3) // remove the node will result an empty route, take care of this special case
            {
                solution.TourList.Remove(routeOfInsertNode);
                solution.TotalCost = solution.TotalCost - originalRouteOfInsertNodeLength;
            }
            else
            {
                routeOfInsertNode.AllNodes.Remove(insertNode);
                routeOfInsertNode.CalculateLength();
                solution.TotalCost = solution.TotalCost + routeOfInsertNode.Length - originalRouteOfInsertNodeLength;
            }

            int index = targetRoute.AllNodes.IndexOf(tail);
            targetRoute.AllNodes.Insert(index + 1, insertNode);
            // Update the onPath property of the insertNode
            insertNode.onPath = targetRoute;
            // Update the route length
            targetRoute.CalculateLength();
            // Update the route load
            targetRoute.Load = targetRoute.Load + insertNode.Demand;
            // Update solution total cost
            solution.TotalCost = solution.TotalCost + targetRoute.Length - originalTargetRouteLength;
        }

        // If RTR is set to be true, we use record-to-record move, otherwise we use downhills move
        // If interRoute is set to be true, moves between two routes are considered, otherwise they are not considered
        // If intraRoute is set to be true, moves within one route are considered,  otherwise they are not considered
        public static void OnePointMove(CVRPSolution currentSolution, bool RTR = true, bool interRoute = true, bool intraRoute = true)
        {
            for (int i = 1; i < nodeList.Count; i++)
            {
                List<Path> tourList = currentSolution.TourList;
                Node insertNode = nodeList[i];
                Path routeOfInsertNode = insertNode.onPath;
                int indexOfInsertNode = routeOfInsertNode.AllNodes.IndexOf(insertNode);

                for (int k = 0; k < tourList.Count; k++)
                {
                    bool localMoveMade = false; // In order to break the loop once the move is made
                    Path targetRoute = tourList[k];

                    for (int j = 0; j < targetRoute.AllNodes.Count - 1; j++)
                    {
                        Node tail = targetRoute.AllNodes[j];
                        Node head = targetRoute.AllNodes[j + 1];

                        // If either tail or head is the same as insertNode, continue to next iteration
                        if (tail == insertNode || head == insertNode)
                        {
                            continue;
                        }

                        // if either tail or head is in the neighbors of insertNode, try to insert insertNode between edge (tail, head)
                        // otherwise, continue to next edge
                        if (neighborList[insertNode].ContainsKey(tail) || neighborList[insertNode].ContainsKey(head))
                        {
                            double insertCost = 0;
                            double removeCost = 0;
                            double savings = 0;

                            
                            // If edge (tail, head) and insertNode are in the same route, and intraRoute == true, try intraRoute moves
                            if ( targetRoute == routeOfInsertNode && intraRoute == true)
                            {
                                insertCost = OnePointInsertCost(insertNode, tail, head);
                                removeCost = OnePointRemoveCost(indexOfInsertNode, routeOfInsertNode);
                                savings = -insertCost - removeCost;

                                // If savings > 0, make the move, no matter the parameter RTR is true or false
                                if (savings > 0.000001) // Do not use 0, the precision problem will make the program running forever
                                {
                                    MakeOnePointIntraRouteMove(insertNode, tail, head, routeOfInsertNode, currentSolution);
                                    UpdateCurrentBestSolution(currentSolution);
                                    localMoveMade = true;
                                    moveMade = true;
                                    break;
                                }

                                // If savings <= 0 and RTR is true and the rtr critiria is satisfied, make the move
                                if (savings <= 0 && RTR == true && currentSolution.TotalCost - savings <= record + deviation)
                                {
                                    MakeOnePointIntraRouteMove(insertNode, tail, head, routeOfInsertNode, currentSolution);
                                    localMoveMade = true;
                                    moveMade = true;
                                    break;
                                }
                            }
                            // If edge (tail, head) and insertNode are not in the same route, and interRoute == true, try interRoute moves
                            if ( targetRoute != routeOfInsertNode && interRoute == true && (targetRoute.Load + insertNode.Demand <= capacity))
                            {
                                insertCost = OnePointInsertCost(insertNode, tail, head);
                                removeCost = OnePointRemoveCost(indexOfInsertNode, routeOfInsertNode);
                                savings = -insertCost - removeCost;

                                // If savings > 0, make the move, no matter the parameter RTR is true or false
                                if (savings > 0)
                                {
                                    MakeOnePointInterRouteMove(insertNode, tail, head, routeOfInsertNode, targetRoute, currentSolution);
                                    UpdateCurrentBestSolution(currentSolution);
                                    localMoveMade = true;
                                    moveMade = true;
                                    break;
                                }

                                // If savings <= 0 and RTR is true and the rtr critiria is satisfied, make the move
                                if (savings <= 0 && RTR == true && currentSolution.TotalCost - savings <= record + deviation)
                                {
                                    MakeOnePointInterRouteMove(insertNode, tail, head, routeOfInsertNode, targetRoute, currentSolution);
                                    localMoveMade = true;
                                    moveMade = true;
                                    break;
                                }
                            }
                        }
                    }

                    if (localMoveMade == true)
                    {
                        break;
                    }
                }
            }
        }



        // calculate the cost of exchanging two nodes from two different routes
        public static double TwoPointInterExchangingCost(int indexOfExchangingNode1, int indexOfExchangingNode2, Path route1, Path route2)
        {
            double exchangingCost = 0;

            int i = indexOfExchangingNode1;
            int j = indexOfExchangingNode2;
            exchangingCost = linkMatrix[route2.AllNodes[j - 1].Id, route2.AllNodes[j].Id].Length + linkMatrix[route2.AllNodes[j].Id, route2.AllNodes[j + 1].Id].Length -
                             linkMatrix[route2.AllNodes[j - 1].Id, route1.AllNodes[i].Id].Length - linkMatrix[route1.AllNodes[i].Id, route2.AllNodes[j + 1].Id].Length +
                             linkMatrix[route1.AllNodes[i - 1].Id, route1.AllNodes[i].Id].Length + linkMatrix[route1.AllNodes[i].Id, route1.AllNodes[i + 1].Id].Length -
                             linkMatrix[route1.AllNodes[i - 1].Id, route2.AllNodes[j].Id].Length - linkMatrix[route2.AllNodes[j].Id, route1.AllNodes[i + 1].Id].Length;

            return exchangingCost;
        }

        // calculate the cost of exchanging two nodes in a route
        public static double TwoPointIntraExchangingCost(int indexOfExchangingNode1, int indexOfExchangingNode2, Path routeOfExchanging)
        {
            double exchangingCost = 0;
            int i = indexOfExchangingNode1;
            int j = indexOfExchangingNode2;
            exchangingCost = linkMatrix[routeOfExchanging.AllNodes[j - 1].Id, routeOfExchanging.AllNodes[j].Id].Length + linkMatrix[routeOfExchanging.AllNodes[j].Id, routeOfExchanging.AllNodes[j + 1].Id].Length -
                             linkMatrix[routeOfExchanging.AllNodes[j - 1].Id, routeOfExchanging.AllNodes[i].Id].Length - linkMatrix[routeOfExchanging.AllNodes[i].Id, routeOfExchanging.AllNodes[j + 1].Id].Length +
                             linkMatrix[routeOfExchanging.AllNodes[i - 1].Id, routeOfExchanging.AllNodes[i].Id].Length + linkMatrix[routeOfExchanging.AllNodes[i].Id, routeOfExchanging.AllNodes[i + 1].Id].Length -
                             linkMatrix[routeOfExchanging.AllNodes[i - 1].Id, routeOfExchanging.AllNodes[j].Id].Length - linkMatrix[routeOfExchanging.AllNodes[j].Id, routeOfExchanging.AllNodes[i + 1].Id].Length;

            return exchangingCost;

        }

        public static void MaketwoPointIntraRouteMove(Node NodeI, Node NodeJ, Path route, CVRPSolution solution)
        {
            double originalRouteLength = route.Length;

            int indexI = route.AllNodes.IndexOf(NodeI);
            int indexJ = route.AllNodes.IndexOf(NodeJ);

            route.AllNodes.Remove(NodeI);
            route.AllNodes.Remove(NodeJ);

            route.AllNodes.Insert(indexI, NodeJ);
            route.AllNodes.Insert(indexJ, NodeI);

            route.CalculateLength();

            // Update solution total cost
            solution.TotalCost = solution.TotalCost + route.Length - originalRouteLength;

        }
        public static void MaketwoPointInterRouteMove(Node NodeI, Node NodeJ, Path routeOfExchangingNodeI, Path routeOfExchangingNodeJ, CVRPSolution solution)
        {
            double originalRouteLengthOfExchangingNodeI = routeOfExchangingNodeI.Length;
            double originalRouteLengthOfExchangingNodeJ = routeOfExchangingNodeJ.Length;

            int indexI = routeOfExchangingNodeI.AllNodes.IndexOf(NodeI);
            int indexJ = routeOfExchangingNodeJ.AllNodes.IndexOf(NodeJ);

            routeOfExchangingNodeI.AllNodes.Remove(NodeI);
            routeOfExchangingNodeJ.AllNodes.Remove(NodeJ);

            solution.TotalCost = solution.TotalCost - originalRouteLengthOfExchangingNodeI - originalRouteLengthOfExchangingNodeJ;

            routeOfExchangingNodeI.AllNodes.Insert(indexI, NodeJ);
            routeOfExchangingNodeJ.AllNodes.Insert(indexJ, NodeI);

            // Update the onPath property of the exchanging Node
            NodeI.onPath = routeOfExchangingNodeJ;
            NodeJ.onPath = routeOfExchangingNodeI;

            // Update the route length

            routeOfExchangingNodeI.CalculateLength();
            routeOfExchangingNodeJ.CalculateLength();

            // Update the route load
            routeOfExchangingNodeI.Load = routeOfExchangingNodeI.Load + NodeJ.Demand;
            routeOfExchangingNodeJ.Load = routeOfExchangingNodeJ.Load + NodeI.Demand;

            // Update solution total cost
            solution.TotalCost = solution.TotalCost + routeOfExchangingNodeI.Length + routeOfExchangingNodeJ.Length;
        }

        public static void TwoPointMove(CVRPSolution currentSolution, bool RTR = true, bool interRoute = true, bool intraRoute = true)
        {
            for (int i = 1; i < nodeList.Count; i++)
            {
                Node nodeInCurrentSolution = nodeList[i];
                Path routeOfExchangingNodeI = nodeInCurrentSolution.onPath;
                int indexOfExchangingNodeI = routeOfExchangingNodeI.AllNodes.IndexOf(nodeInCurrentSolution);

                if ((routeOfExchangingNodeI.AllNodes.Count - indexOfExchangingNodeI) < 3)
                {
                    return;
                }

                List<Node> neighborsOfNodeI = new List<Node>(neighborList[nodeInCurrentSolution].Keys);
                foreach (Node nodeJ in neighborsOfNodeI)
                {
                    bool localMoveMade = false;
                    Path routeOfExchangingNodeJ = nodeJ.onPath;
                    double exChangingCost = 0;
                    int indexOfExchangingNodeJ = routeOfExchangingNodeJ.AllNodes.IndexOf(nodeJ);

                    if ((routeOfExchangingNodeJ.AllNodes.Count - indexOfExchangingNodeJ) < 3)
                    {
                        return;
                    }

                    if(routeOfExchangingNodeI != routeOfExchangingNodeJ && interRoute == true && (routeOfExchangingNodeI.Load + nodeJ.Demand <= capacity) && (routeOfExchangingNodeJ.Load + nodeInCurrentSolution.Demand <= capacity))
                    {
                        exChangingCost = TwoPointInterExchangingCost(indexOfExchangingNodeI, indexOfExchangingNodeJ, routeOfExchangingNodeI, routeOfExchangingNodeJ);
                        if (exChangingCost > 0.000001)
                        {
                              MaketwoPointInterRouteMove(nodeInCurrentSolution, nodeJ, routeOfExchangingNodeI, routeOfExchangingNodeJ, currentSolution);
                              UpdateCurrentBestSolution(currentSolution);
                              localMoveMade = true;
                              moveMade = true;
                              break;
                        }
                        // If exChangingCosts <= 0 and RTR is true and the rtr critiria is satisfied, make the move
                        if (exChangingCost <= 0 && RTR == true && currentSolution.TotalCost - exChangingCost <= record + deviation)
                        {
                              MaketwoPointInterRouteMove(nodeInCurrentSolution, nodeJ, routeOfExchangingNodeI, routeOfExchangingNodeJ, currentSolution);
                              localMoveMade = true;
                              moveMade = true;
                              break;
                        }
                    }

                    if (routeOfExchangingNodeI == routeOfExchangingNodeJ && intraRoute == true)
                    {
                        exChangingCost = TwoPointIntraExchangingCost(indexOfExchangingNodeI, indexOfExchangingNodeJ, routeOfExchangingNodeI);
                        if (exChangingCost > 0.000001)
                        {
                              MaketwoPointIntraRouteMove(nodeInCurrentSolution, nodeJ, routeOfExchangingNodeI, currentSolution);
                              UpdateCurrentBestSolution(currentSolution);
                              localMoveMade = true;
                              moveMade = true;
                              break;
                        }
                        // If exChangingCosts <= 0 and RTR is true and the rtr critiria is satisfied, make the move
                        if (exChangingCost <= 0 && RTR == true && currentSolution.TotalCost - exChangingCost <= record + deviation)
                        {
                              localMoveMade = true;
                              moveMade = true;
                              break;
                        }
                    }
                    if (localMoveMade == true)
                    {
                        break;
                    }
                }
            }
        }

        // Calculate the cost of two opt move
        public static double TwoOptCost(Node nodeI, Path routeOfNodeI, int indexOfNodeI, Node nodeJ, Path routeOfNodeJ, int indexOfNodeJ)
        {
            double cost = 0;

            if (routeOfNodeI == routeOfNodeJ)
            {
                int minIndex = Math.Min(indexOfNodeI, indexOfNodeJ);
                int maxIndex = Math.Max(indexOfNodeI, indexOfNodeJ);

                cost = linkMatrix[routeOfNodeI.AllNodes[minIndex - 1].Id, routeOfNodeI.AllNodes[maxIndex].Id].Length
                    + linkMatrix[routeOfNodeI.AllNodes[minIndex].Id, routeOfNodeI.AllNodes[maxIndex + 1].Id].Length
                    - linkMatrix[routeOfNodeI.AllNodes[minIndex - 1].Id, routeOfNodeI.AllNodes[minIndex].Id].Length
                    - linkMatrix[routeOfNodeI.AllNodes[maxIndex].Id, routeOfNodeI.AllNodes[maxIndex + 1].Id].Length;
            }
            else 
            {
                cost = linkMatrix[routeOfNodeI.AllNodes[indexOfNodeI - 1].Id, nodeJ.Id].Length
                    + linkMatrix[routeOfNodeJ.AllNodes[indexOfNodeJ - 1].Id, nodeI.Id].Length
                    - linkMatrix[routeOfNodeI.AllNodes[indexOfNodeI - 1].Id, nodeI.Id].Length
                    - linkMatrix[routeOfNodeJ.AllNodes[indexOfNodeJ - 1].Id, nodeJ.Id].Length;
            }

            return cost;
        }

        public static void MakeTwoOptMove(Node nodeI, Path routeOfNodeI, int indexOfNodeI, Node nodeJ, Path routeOfNodeJ, int indexOfNodeJ, CVRPSolution solution)
        {

            if (routeOfNodeI == routeOfNodeJ)
            {
                int minIndex = Math.Min(indexOfNodeI, indexOfNodeJ);
                int maxIndex = Math.Max(indexOfNodeI, indexOfNodeJ);
                double originalRouteLength = routeOfNodeI.Length;
                List<Node> listOfNode = new List<Node>();

                for (int i = 0; i < minIndex; i++)
                {
                    listOfNode.Add(routeOfNodeI.AllNodes[i]);
                }
                for (int i = maxIndex; i >= minIndex; i--)
                {
                    listOfNode.Add(routeOfNodeI.AllNodes[i]);
                }
                for (int i = maxIndex + 1; i < routeOfNodeI.AllNodes.Count; i++)
                {
                    listOfNode.Add(routeOfNodeI.AllNodes[i]);
                }

                if (routeOfNodeI.AllNodes.Count != listOfNode.Count)
                {
                    Console.WriteLine("error");
                    Console.ReadKey();
                }

                Path newRoute = new Path(listOfNode);
                solution.TourList.Remove(routeOfNodeI);
                solution.TourList.Add(newRoute);
                solution.TotalCost = solution.TotalCost + newRoute.Length - originalRouteLength;
            }
            else
            {
                double originalRouteLengthI = routeOfNodeI.Length;
                double originalRouteLengthJ = routeOfNodeJ.Length;
                List<Node> listOfNodeI = new List<Node>();
                List<Node> listOfNodeJ = new List<Node>();

                for (int i = 0; i < indexOfNodeI; i++)
                {
                    listOfNodeI.Add(routeOfNodeI.AllNodes[i]);
                }
                for (int i = indexOfNodeJ; i < routeOfNodeJ.AllNodes.Count; i++)
                {
                    listOfNodeI.Add(routeOfNodeJ.AllNodes[i]);
                }
                for (int i = 0; i < indexOfNodeJ; i++)
                {
                    listOfNodeJ.Add(routeOfNodeJ.AllNodes[i]);
                }
                for (int i = indexOfNodeI; i < routeOfNodeI.AllNodes.Count; i++)
                {
                    listOfNodeJ.Add(routeOfNodeI.AllNodes[i]);
                }

                Path newRouteI = null;
                Path newRouteJ = null;
                if (listOfNodeI.Count > 2)
                {
                    newRouteI = new Path(listOfNodeI);
                }
                if (listOfNodeJ.Count > 2)
                {
                    newRouteJ = new Path(listOfNodeJ);
                }

                solution.TourList.Remove(routeOfNodeI);
                solution.TourList.Remove(routeOfNodeJ);
                solution.TotalCost = solution.TotalCost - originalRouteLengthI - originalRouteLengthJ;
                if (newRouteI != null)
                {
                    solution.TourList.Add(newRouteI);
                    solution.TotalCost = solution.TotalCost + newRouteI.Length;
                }
                if (newRouteJ != null)
                {
                    solution.TourList.Add(newRouteJ);
                    solution.TotalCost = solution.TotalCost + newRouteJ.Length;
                }
            }

        }

        public static void TwoOptMove(CVRPSolution currentSolution, bool RTR = true, bool interRoute = true, bool intraRoute = true)
        {
            for (int i = 1; i < nodeList.Count; i++)
            {
                Node nodeI = nodeList[i];
                Path routeOfNodeI = nodeI.onPath;
                int indexOfNodeI = routeOfNodeI.AllNodes.IndexOf(nodeI);
                List<Node> neighborsOfNodeI = new List<Node>(neighborList[nodeI].Keys);
                foreach (Node nodeJ in neighborsOfNodeI)
                {
                    double twoOptCost = 0;
                    double twoOptSavings = 0;

                    Path routeOfNodeJ = nodeJ.onPath;
                    int indexOfNodeJ = routeOfNodeJ.AllNodes.IndexOf(nodeJ);
                    if (routeOfNodeI == routeOfNodeJ && intraRoute == true)
                    {
                        twoOptCost = TwoOptCost(nodeI, routeOfNodeI, indexOfNodeI, nodeJ, routeOfNodeJ, indexOfNodeJ);
                        twoOptSavings = -twoOptCost;

                        if (twoOptSavings > 0.000001)
                        {

                            MakeTwoOptMove(nodeI, routeOfNodeI, indexOfNodeI, nodeJ, routeOfNodeJ, indexOfNodeJ, currentSolution);
                            UpdateCurrentBestSolution(currentSolution);
                            moveMade = true;
                            break;
                        }

                        if (twoOptSavings <= 0 && RTR == true && currentSolution.TotalCost - twoOptSavings <= record + deviation)
                        {
                            MakeTwoOptMove(nodeI, routeOfNodeI, indexOfNodeI, nodeJ, routeOfNodeJ, indexOfNodeJ, currentSolution);
                            moveMade = true;
                            break;
                        }
                    }

                    if (routeOfNodeI != routeOfNodeJ && interRoute == true)
                    {
                        double loadOfNewRouteI = 0;
                        double loadOfNewRouteJ = 0;

                        for (int k = 1; k < routeOfNodeI.AllNodes.Count - 1; k++)
                        {
                            if (k < indexOfNodeI)
                            {
                                loadOfNewRouteI += routeOfNodeI.AllNodes[k].Demand;
                            }
                            else
                            {
                                loadOfNewRouteJ += routeOfNodeI.AllNodes[k].Demand;
                            }
                        }

                        for (int k = 1; k < routeOfNodeJ.AllNodes.Count - 1; k++)
                        {
                            if (k < indexOfNodeJ)
                            {
                                loadOfNewRouteJ += routeOfNodeJ.AllNodes[k].Demand;
                            }
                            else
                            {
                                loadOfNewRouteI += routeOfNodeJ.AllNodes[k].Demand;
                            }
                        }

                        // If either new route violates the capacity contraint, continue to next iteration
                        if (loadOfNewRouteI > capacity || loadOfNewRouteJ > capacity)
                        {
                            continue;
                        }
                        // else do the following 
                        twoOptCost = TwoOptCost(nodeI, routeOfNodeI, indexOfNodeI, nodeJ, routeOfNodeJ, indexOfNodeJ);
                        twoOptSavings = -twoOptCost;

                        if (twoOptSavings > 0.000001)
                        {
                            MakeTwoOptMove(nodeI, routeOfNodeI, indexOfNodeI, nodeJ, routeOfNodeJ, indexOfNodeJ, currentSolution);
                            UpdateCurrentBestSolution(currentSolution);
                            moveMade = true;
                            break;
                        }

                        if (twoOptSavings <= 0 && RTR == true && currentSolution.TotalCost - twoOptSavings <= record + deviation)
                        {
                            MakeTwoOptMove(nodeI, routeOfNodeI, indexOfNodeI, nodeJ, routeOfNodeJ, indexOfNodeJ, currentSolution);
                            moveMade = true;
                            break;
                        }
                    }
                }
            }
        }
    }
}